/*
	Written By Pradipna Nepal and Sanjay Giri
	www.pradsprojects.com

	Copyright (C) 2011 Prads Projects
	Please read COPYING.txt included along with this source code for more detail.
	If not included, see http://www.gnu.org/licenses/
*/

#include "libEmuCPUCore.h"

CPUCore::CPUCore(HWND hwnd, bool *startEmulation) {
	cycle = 0;
	regs.PC = 0;
	regs.flag = 0;
	regs.SP = 0xFDF0;
	for (int i = 0; i < 16; ++i) regs.R[i] = 0;
	memset(cMem, 0, 0x10000);

	*startEmulation = true;
	if (!graphics.initialize(hwnd, 320, 240)) {
		MessageBox(hwnd, "Error Initializing DirectX", "Error", MB_ICONERROR);
		*startEmulation = false;
		return;
	}
	if (!loadROM(cMem)) {
		MessageBox(hwnd, "Error Loading ROM", "Error", MB_ICONERROR);
		*startEmulation = false;
	}
}

bool CPUCore::checkCondition(unsigned char *condition) {
	switch (*condition & 0xF) {
		case 0:		//Z
			if (regs.flag & FLAG_Z) return true;
			break;
		case 1:		//NZ
			if (!(regs.flag & FLAG_Z)) return true;
			break;
		case 2:		//N
			if (regs.flag & FLAG_N) return true;
			break;
		case 3:		//NN
			if (!(regs.flag & FLAG_N)) return true;
			break;
		case 4:		//P
			if (!(regs.flag & FLAG_N) && !(regs.flag & FLAG_Z)) return true;
			break;
		case 5:		//O
			if (regs.flag & FLAG_O) return true;
			break;
		case 6:		//NO
			if (!(regs.flag & FLAG_O)) return true;
			break;
		case 7:		//A
			if (!(regs.flag & FLAG_C) && !(regs.flag & FLAG_Z)) return true;
			break;
		case 8:		//AE
			if (!(regs.flag & FLAG_C)) return true;
			break;
		case 9:		//B
			if (regs.flag & FLAG_C) return true;
			break;
		case 0xA:	//BE
			if ((regs.flag & FLAG_C) && (regs.flag & FLAG_Z)) return true;
			break;
		case 0xB:	//G
			if (((regs.flag & FLAG_O) == (regs.flag & FLAG_N)) && !(regs.flag & FLAG_Z)) return true;
			break;
		case 0xC:	//GE
			if ((regs.flag & FLAG_O) == (regs.flag & FLAG_N)) return true;
			break;
		case 0xD:	//L
			if ((regs.flag & FLAG_O) != (regs.flag & FLAG_N)) return true;
			break;
		case 0xE:	//LE
			if (((regs.flag & FLAG_O) != (regs.flag & FLAG_N)) || (regs.flag & FLAG_Z)) return true;
			break;
	}

	return false;
}

void CPUCore::pushPCtoStack() {
	writeMemory(regs.PC, regs.SP);
	++regs.SP;
	writeMemory(regs.PC >> 8, regs.SP);
	++regs.SP;
}

void CPUCore::addSubUpdateFlags(unsigned int *result, int *sign) {
	if (*result == 0) 
		regs.flag |= FLAG_Z;
	else if (*result > 0xFFFF) {
		regs.flag |= FLAG_C;
		*result &= 0xFFFF;
	}
	if (*sign != DONT_CHECK) {
		if (*sign == NEGATIVE) { 
			if (*result & 0x8000) {
				regs.flag |= FLAG_O;
				regs.flag |= FLAG_N;
				return;
			}
		} else if (!(*result & 0x8000)) {
			regs.flag |= FLAG_O;
		}
	}
	if (*result & 0x8000) regs.flag |= FLAG_N;
}

void CPUCore::updateZandN(unsigned int *result) {
	regs.flag &= 0x7B;	//Clear Z and N Flags
	if (*result == 0) 
		regs.flag |= FLAG_Z;
	else if (*result & 0x8000) 
		regs.flag |= FLAG_N;
}

unsigned int CPUCore::getPC() { return regs.PC; }

unsigned int CPUCore::getSP() { return regs.SP; }

unsigned char CPUCore::getFlag() { return regs.flag; }

unsigned int CPUCore::getR(int index) { return regs.R[index]; }

unsigned char *CPUCore::getMemory() { return cMem; }

unsigned int CPUCore::getCycle() { return cycle; }

Inputs *CPUCore::getInput() { return &inputs; }

void CPUCore::setPC(int val) { regs.PC = val & 0xFFFF; }

void CPUCore::setSP(int val) { regs.SP = val & 0xFFFF; }

void CPUCore::setFlag(unsigned char val) { regs.flag = val; }

void CPUCore::setR(int val, int index) { regs.R[index] = val & 0xFFFF; }

void CPUCore::setMemory(unsigned char val, int addr) { cMem[addr] = val; }

//All memory writes from chip16 programs MUST go through this function
void CPUCore::writeMemory(unsigned char val, int addr) {
	//Why some games are writing on Read Only Memory address, I have no idea!!!
	//if (addr < romSize) return;
	if (addr > 0xFFFF) return;
	cMem[addr] = val;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////GRAPHICS INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//01 00 00 00	CLS			Clear screen (Foreground layer is erased, background is set to index 0).
void CPUCore::CLS() {
	graphics.clearScreen();
}

//02 00 00 00	VBLNK			Wait for VBlank. If (!vblank) PC-=4;
void CPUCore::VBLNK() {
	while (!graphics.checkIfVBlank(&cycle)) ++cycle;
}

//03 00 0N 00	BGC N			Set background color to index N. If the index was set to 0 the color is black.
void CPUCore::BGCN() {
	graphics.setBackground(&cMem[regs.PC - 2]);
}

//04 00 LL HH	SPR HHLL		Set sprite width (LL) and height (HH).
void CPUCore::SPRHHLL() {
	unsigned char width = cMem[regs.PC - 2];
	unsigned char height = cMem[regs.PC - 1];
	graphics.setSpriteSize(&width, &height);
}

//05 YX LL HH	DRW RX, RY, HHLL	Draw sprite from address HHLL at coordinates stored in register X and Y (treated as signed numbers). Affects carry flag (explained in GPU info).
void CPUCore::DRWRXRYHHLL() {
	short int x =  regs.R[cMem[regs.PC - 3] & 0xF];
	short int y =  regs.R[cMem[regs.PC - 3] >> 4];
	int address =  (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
	graphics.drawSprite(cMem, x, y, address, &regs.flag);
}

//06 YX 0Z 00	DRW RX, RY, RZ		Draw sprite from address pointed by register Z at coordinates stored in register X and Y (treated as signed numbers).  Affects carry flag.
void CPUCore::DRWRXRYRZ() {
	short int x =  regs.R[cMem[regs.PC - 3] & 0xF];
	short int y =  regs.R[cMem[regs.PC - 3] >> 4];
	int address =  regs.R[cMem[regs.PC - 2] & 0xF];
	graphics.drawSprite(cMem, x, y, address, &regs.flag);
}

//08 00 00 00	FLIP 0, 0		Set flip orientation for sprites. horizontal flip = false; vertical flip = false
//08 00 00 01	FLIP 0, 1		Set flip orientation for sprites. horizontal flip = false; vertical flip = true
//08 00 00 02	FLIP 1, 0		Set flip orientation for sprites. horizontal flip = true;  vertical flip = false
//08 00 00 03	FLIP 1, 1		Set flip orientation for sprites. horizontal flip = true;  vertical flip = true
void CPUCore::FLIP() {
	switch (cMem[regs.PC - 1] & 0xF) {
		case 0:
			graphics.setFlip(false, false);
			break;
		case 1:
			graphics.setFlip(false, true);
			break;
		case 2:
			graphics.setFlip(true, false);
			break;
		case 3:
			graphics.setFlip(true, true);
	}	
}

//D0 00 LL HH     PAL HHLL   Load the palette starting at address HHLL, 16*3 bytes, RGB format; used for all drawing since last vblank.
void CPUCore::PALHHLL() {
	graphics.loadPalette(cMem, (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2]);
}

//D1 0x 00 00     PAL Rx   Load the palette starting at the address pointed to by Register X, 16*3 bytes, RGB format; used for all drawing since last vblank.
void CPUCore::PALRx() {
	graphics.loadPalette(cMem, regs.R[cMem[regs.PC - 3] & 0xF]);
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////RAND INSTRUCTION//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//07 0X LL HH	RND RX, HHLL		Generate a random number and store it in register X. Maximum value is HHLL.
void CPUCore::RNDRXHHLL() {
	int max =(cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];	
	int randomNum = rand() ^ (rand() << 1);	//16 bit ramdom value since RAND_MAX is usually 0x7fff
	regs.R[cMem[regs.PC - 3] & 0xF] = randomNum % (max + 1); 
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////JUMP INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//10 00 LL HH	JMP HHLL		Jump to the specified address.
void CPUCore::JMPHHLL() {
	regs.PC = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
}

//12 0x LL HH	Jx  HHLL		Jump to the specified address if condition 'x' (x refers to a Conditional Type)
void CPUCore::JxHHLL() {
	if (checkCondition(&cMem[regs.PC - 3])) JMPHHLL();
}

//13 YX LL HH	JME RX, RY, HHLL	Jump to the specified address if value in register X is equal to value in register Y.
void CPUCore::JMERXRYHHLL() {
	if (regs.R[cMem[regs.PC - 3] & 0xF] == regs.R[cMem[regs.PC - 3] >> 4]) JMPHHLL();
}

//16 0X 00 00	JMP RX			Jump to the address specified in register X (Indirect Jump)
void CPUCore::JMPRX() {
	regs.PC = regs.R[cMem[regs.PC - 3] & 0xF];
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////CALL INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//14 00 LL HH	CALL HHLL		Call subroutine at the specified address. Store PC to [SP]. Increase SP by 2.
void CPUCore::CALLHHLL() {
	pushPCtoStack();
	JMPHHLL();
}

//15 00 00 00	RET			Return from a subroutine. Decrease SP by 2. Get PC from [SP].
void CPUCore::RET() {
	regs.SP -= 2;
	regs.PC = (cMem[regs.SP + 1] << 8) | cMem[regs.SP];
}

//17 0x LL HH     Cx   HHLL		If condition 'x', then performs a CALL; Else, does nothing. (x refers to a Conditional Type).
void CPUCore::CxHHLL() {
	if (checkCondition(&cMem[regs.PC - 3])) CALLHHLL();
}

//18 0X 00 00	CALL RX			Call subroutine at the address specified in RX. Store PC to [SP]. Increase SP by 2.
void CPUCore::CALLRX() {
	pushPCtoStack();
	regs.PC = regs.R[cMem[regs.PC - 3] & 0xF];
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////LOAD INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//20 0X LL HH	LDI RX, HHLL		Load immediate value to register X.
void CPUCore::LDIRXHHLL() {
	regs.R[cMem[regs.PC - 3] & 0xF] = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
}

//21 00 LL HH	LDI SP, HHLL		Point SP to the specified address. Does not move existing values in memory to new location.
void CPUCore::LDISPHHLL() {
	regs.SP = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
}

//22 0X LL HH	LDM RX, HHLL		Load register X with the 16bit value at the specified address.
void CPUCore::LDMRXHHLL() {
	int addr = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
	regs.R[cMem[regs.PC - 3] & 0xF] = (cMem[addr + 1] << 8) | cMem[addr];
}

//23 YX 00 00	LDM RX, RY		Load register X with the 16bit value at the specified address pointed by register Y.
void CPUCore::LDMRXRY() {
	int addr = regs.R[cMem[regs.PC - 3] >> 4];
	regs.R[cMem[regs.PC - 3] & 0xF] = (cMem[addr + 1] << 8) | cMem[addr];
}

//24 YX 00 00	MOV RX, RY		Copy data from register Y to register X.
void CPUCore::MOVRXRY() {
	regs.R[cMem[regs.PC - 3] & 0xF] = regs.R[cMem[regs.PC - 3] >> 4];
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////STORE INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//30 0X LL HH	STM RX, HHLL		Store value of register X at the specified address.
void CPUCore::STMRXHHLL() {
	int addr = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2]; 
	
	writeMemory(regs.R[cMem[regs.PC - 3] & 0xF], addr);
	writeMemory(regs.R[cMem[regs.PC - 3] & 0xF] >> 8, addr + 1);
}

//31 YX 00 00	STM RX, RY		Store value of register X at the specified address pointed by register Y.
void CPUCore::STMRXRY() {
	writeMemory(regs.R[cMem[regs.PC - 3] & 0xF], regs.R[cMem[regs.PC - 3] >> 4]);
	writeMemory(regs.R[cMem[regs.PC - 3] & 0xF] >> 8, regs.R[cMem[regs.PC - 3] >> 4] + 1);
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////ADD INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//Check sign of two numbers
void CPUCore::addCheckSign(unsigned int *first, unsigned int *second, int *signOut) {
	if ((*first & 0x8000) && (*second & 0x8000))
		*signOut = POSITIVE;
	else if (!(*first & 0x8000) && !(*second & 0x8000))
		*signOut = NEGATIVE;
	else
		*signOut = DONT_CHECK;
}

//40 0X LL HH	ADDI RX, HHLL		Add immediate value to register X. Affects [c,z,o,n]
void CPUCore::ADDIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF, sign;
	unsigned int HHLL = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
	regs.flag = 0;
	
	addCheckSign(&regs.R[regID], &HHLL, &sign);
	regs.R[regID] += HHLL;
	addSubUpdateFlags(&regs.R[regID], &sign);
}

//41 YX 00 00	ADD RX, RY		Add value of register Y to register X. Result is stored in register X. Affects [c,z,o,n]
void CPUCore::ADDRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4;
	int sign;
	regs.flag = 0;

	addCheckSign(&regs.R[xID], &regs.R[yID], &sign);
	regs.R[xID] += regs.R[yID];
	addSubUpdateFlags(&regs.R[xID], &sign);
}

//42 YX 0Z 00	ADD RX, RY, RZ		Add value of register Y to register X. Result is stored in register Z. Affects [c,z,o,n]
void CPUCore::ADDRXRYRZ() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4, zID = cMem[regs.PC - 2] & 0xF;
	int sign;
	regs.flag = 0;

	addCheckSign(&regs.R[xID], &regs.R[yID], &sign);
	regs.R[zID] = regs.R[xID] + regs.R[yID];
	addSubUpdateFlags(&regs.R[zID], &sign);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////SUBTRACT INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//Check sign of two numbers
void CPUCore::subCheckSign(unsigned int *first, unsigned int *second, int *signOut) {
	if ((*first & 0x8000) && !(*second & 0x8000))
		*signOut = POSITIVE;
	else if (!(*first & 0x8000) && (*second & 0x8000))
		*signOut = NEGATIVE;
	else
		*signOut = DONT_CHECK;
}

//50 0X LL HH	SUBI RX, HHLL		Subtract immediate value from register X. Result is stored in register X. Affects [c,z,o,n].
void CPUCore::SUBIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF, sign; 
	unsigned int HHLL = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
	regs.flag = 0;
	
	subCheckSign(&regs.R[regID], &HHLL, &sign);
	regs.R[regID] -= HHLL;
	addSubUpdateFlags(&regs.R[regID], &sign);
}

//51 YX 00 00	SUB RX, RY		Subtract value of register Y from register X. Result is stored in register X. Affects [c,z,o,n]
void CPUCore::SUBRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4;
	int sign;
	regs.flag = 0;

	subCheckSign(&regs.R[xID], &regs.R[yID], &sign);
	regs.R[xID] -= regs.R[yID];
	addSubUpdateFlags(&regs.R[xID], &sign);
}

//52 YX 0Z 00	SUB RX, RY, RZ		Subtract value of register Y from register X. Result is stored in register Z. Affects [c,z,o,n]
void CPUCore::SUBRXRYRZ() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4, zID = cMem[regs.PC - 2] & 0xF;
	int sign;
	regs.flag = 0;

	subCheckSign(&regs.R[xID], &regs.R[yID], &sign);
	regs.R[zID] = regs.R[xID] - regs.R[yID];
	addSubUpdateFlags(&regs.R[zID], &sign);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////COMPARE INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//53 0X LL HH	CMPI RX, HHLL		Subtract immediate value from register X. Result is discarded. Affects [c,z,o,n].
void CPUCore::CMPIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF, sign;
	unsigned int HHLL = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
	regs.flag = 0;

	subCheckSign(&regs.R[regID], &HHLL, &sign);
	unsigned int result = regs.R[regID] - HHLL;
	addSubUpdateFlags(&result, &sign);
}

//54 YX 00 00	CMP RX, RY		Subtract value of register Y from register X. Result is discarded. Affects [c,z,o,n]
void CPUCore::CMPRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4;
	int sign;
	regs.flag = 0;

	subCheckSign(&regs.R[xID], &regs.R[yID], &sign);
	unsigned int result = regs.R[xID] - regs.R[yID];
	addSubUpdateFlags(&result, &sign);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////AND INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//60 0X LL HH	ANDI RX, HHLL		AND immediate value with register X. Result is stored in register X. Affects [z,n]
void CPUCore::ANDIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF;

	regs.R[regID] &= ((cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2]);
	updateZandN(&regs.R[regID]);
}

//61 YX 00 00	AND RX, RY		AND value of register Y with value of register X. Result is stored in register X. Affects [z,n]
void CPUCore::ANDRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF;

	regs.R[xID] &= regs.R[cMem[regs.PC - 3] >> 4];
	updateZandN(&regs.R[xID]);
}

//62 YX 0Z 00	AND RX, RY, RZ		AND value of register Y with value of register X. Result is stored in register Z. Affects [z,n]
void CPUCore::ANDRXRYRZ() {
	int zID = cMem[regs.PC - 2] & 0xF;

	regs.R[zID] = regs.R[cMem[regs.PC - 3] & 0xF] & regs.R[cMem[regs.PC - 3] >> 4];
	updateZandN(&regs.R[zID]);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////TEST INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//63 0X LL HH	TSTI RX, HHLL		AND immediate value with register X. Result is discarded. Affects [z,n]
void CPUCore::TSTIRXHHLL() {
	unsigned int result = regs.R[cMem[regs.PC - 3] & 0xF] & ((cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2]);
	updateZandN(&result);
}

//64 YX 00 00	TST RX, RY		AND value of register Y with value of register X. Result is discarded. Affects [z,n]
void CPUCore::TSTRXRY() {
	unsigned int result = regs.R[cMem[regs.PC - 3] & 0xF] & regs.R[cMem[regs.PC - 3] >> 4];
	updateZandN(&result);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////OR INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//70 0X LL HH	ORI RX, HHLL		OR immediate value with register X. Result is stored in register X. Affects [z,n]
void CPUCore::ORIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF;
	
	regs.R[regID] |= ((cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2]);
	updateZandN(&regs.R[regID]);
}

//71 YX 00 00	OR RX, RY		OR value of register Y with value of register X. Result is stored in register X. Affects [z,n]
void CPUCore::ORRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF;
	
	regs.R[xID] |= regs.R[cMem[regs.PC - 3] >> 4];
	updateZandN(&regs.R[xID]);
}

//72 YX 0Z 00	OR RX, RY, RZ		OR value of register Y with value of register X. Result is stored in register Z. Affects [z,n]
void CPUCore::ORRXRYRZ() {
	int zID = cMem[regs.PC - 2] & 0xF;
	
	regs.R[zID] = regs.R[cMem[regs.PC - 3] >> 4] | regs.R[cMem[regs.PC - 3] >> 4];
	updateZandN(&regs.R[zID]);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////XOR INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//80 0X LL HH	XORI RX, HHLL		XOR immediate value with register X. Result is stored in register X. Affects [z,n]
void CPUCore::XORIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF;

	regs.R[regID] ^= ((cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2]);
	updateZandN(&regs.R[regID]);
}

//81 YX 00 00	XOR RX, RY		XOR value of register Y with value of register X. Result is stored in register X. Affects [z,n]
void CPUCore::XORRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF;

	regs.R[xID] ^= regs.R[cMem[regs.PC - 3] >> 4];
	updateZandN(&regs.R[xID]);
}

//82 YX 0Z 00	XOR RX, RY, RZ		XOR value of register Y with value of register X. Result is stored in register Z. Affects [z,n]
void CPUCore::XORRXRYRZ() {
	int zID = cMem[regs.PC - 2] & 0xF;

	regs.R[zID] = regs.R[cMem[regs.PC - 3] & 0xF] ^ regs.R[cMem[regs.PC - 3] >> 4];
	updateZandN(&regs.R[zID]);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////MULTIPLY INSTRUCTIONS////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void CPUCore::setFlagsForMultiply(unsigned int *result){
	updateZandN(result);
	//C
	regs.flag &= 0xFD;	//Clear C flag
	if (*result > 0xFFFF) {
		regs.flag |= FLAG_C;
		*result &= 0xFFFF;
	}
}

//90 0X LL HH	MULI RX, HHLL		Multiply immediate value with register X. Result is stored in register X. Affects [c,z,n]
void CPUCore::MULIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF;
	unsigned int HHLL = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
	
	regs.R[regID] = HHLL * regs.R[regID];
	setFlagsForMultiply(&regs.R[regID]);
}

//91 YX 00 00	MUL RX, RY		Multiply value of register Y with value of register X. Result is stored in register X. Affects [c,z,n]
void CPUCore::MULRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4;
	
	regs.R[xID] = regs.R[xID] * regs.R[yID];;
	setFlagsForMultiply(&regs.R[xID]);
}

//92 YX 0Z 00	MUL RX, RY, RZ		Multiply value of register Y with value of register X. Result is stored in register Z. Affects [c,z,n]
void CPUCore::MULRXRYRZ() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4, zID = cMem[regs.PC - 2] & 0xF;
	
	regs.R[zID] = regs.R[xID] * regs.R[yID];
	setFlagsForMultiply(&regs.R[zID]);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////DIVIDE INSTRUCTIONS////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//A0 0X LL HH	DIVI RX, HHLL		Divide immediate value with register X. Result is stored in register X. Affects [c,z,n]
void CPUCore::DIVIRXHHLL() {
	int regID = cMem[regs.PC - 3] & 0xF;
	
	unsigned int first = (cMem[regs.PC - 1] << 8) | cMem[regs.PC - 2];
	if (first == 0) return;
	unsigned int second = regs.R[regID];
	
	regs.R[regID] = second / first;
	
	regs.flag &= 0xFD;	//Clear C flag
	if ((second % first) != 0) regs.flag |= FLAG_C;
	updateZandN(&regs.R[regID]);
}

//A1 YX 00 00	DIV RX, RY		Divide value of register Y with value of register X. Result is stored in register X. Affects [c,z,n]
void CPUCore::DIVRXRY() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4;
	
	unsigned int first = regs.R[xID];
	unsigned int second = regs.R[yID];
	if (second == 0) return;
	
	regs.R[xID] = first / second;

	regs.flag &= 0xFD;	//Clear C flag
	if ((first % second) != 0) regs.flag |= FLAG_C;
	updateZandN(&regs.R[xID]);
}

//A2 YX 0Z 00	DIV RX, RY, RZ		Divide value of register Y with value of register X. Result is stored in register Z. Affects [c,z,n]
void CPUCore::DIVRXRYRZ() {
	int xID = cMem[regs.PC - 3] & 0xF, yID = cMem[regs.PC - 3] >> 4, zID = cMem[regs.PC - 2] & 0xF;
	
	unsigned int first = regs.R[xID];
	unsigned int second = regs.R[yID];
	if (second == 0) return;

	regs.R[zID] = first / second;

	regs.flag &= 0xFD;	//Clear C flag
	if ((first % second) != 0) regs.flag |= FLAG_C;
	updateZandN(&regs.R[zID]);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////SHIFT INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//B0 0X 0N 00	SHL RX, N		Logical    Shift value in register X left N times.  Affects [z,n]
void CPUCore::SHLRXN() {
	int regID = cMem[regs.PC - 3] & 0xF;

	regs.R[regID] <<= (cMem[regs.PC - 2] & 0xF);
	regs.R[regID] &= 0xFFFF;
	updateZandN(&regs.R[regID]);
}

//B1 0X 0N 00	SHR RX, N		Logical    Shift value in register X right N times. Affects [z,n]
void CPUCore::SHRRXN() {
	int regID = cMem[regs.PC - 3] & 0xF;

	regs.R[regID] >>= (cMem[regs.PC - 2] & 0xF);
	updateZandN(&regs.R[regID]);
}

//B0 0X 0N 00	SAL RX, N		Arithmetic Shift value in register X left N times.  Affects [z,n] (same as SHL)
void CPUCore::SALRXN() {
	SHLRXN();
}

//B2 0X 0N 00	SAR RX, N		Arithmetic Shift value in register X right N times. Affects [z,n]
void CPUCore::SARRXN() {
	int regID = cMem[regs.PC - 3] & 0xF;
	int result = regs.R[regID];

	if (result & 0x8000) result |= 0xFFFF0000;
	result >>= (cMem[regs.PC - 2] & 0xF);
	regs.R[regID] = result & 0xFFFF;
	updateZandN(&regs.R[regID]);
}

//B3 YX 00 00	SHL RX, RY		Logical    Shift value in register X left by the value in (RY & 0xf).  Affects [z,n]
void CPUCore::SHLRXRY() {
	int regID = cMem[regs.PC - 3] & 0xF;

	regs.R[regID] <<= (regs.R[cMem[regs.PC - 3] >> 4] & 0xF);
	regs.R[regID] &= 0xFFFF;
	updateZandN(&regs.R[regID]);
}

//B4 YX 00 00	SHR RX, RY		Logical    Shift value in register X right by the value in (RY & 0xf). Affects [z,n]
void CPUCore::SHRRXRY() {
	int regID = cMem[regs.PC - 3] & 0xF;

	regs.R[regID] >>= (regs.R[cMem[regs.PC - 3] >> 4] & 0xF);
	updateZandN(&regs.R[regID]);
}

//B3 YX 00 00	SAL RX, RY		Arithmetic Shift value in register X left by the value in (RY & 0xf).  Affects [z,n] (same as SHL)
void CPUCore::SALRXRY() {
	SHLRXRY();
}

//B5 YX 00 00	SAR RX, RY		Arithmetic Shift value in register X right by the value in (RY & 0xf). Affects [z,n]
void CPUCore::SARRXRY() {
	int regID = cMem[regs.PC - 3] & 0xF;
	int result = regs.R[regID];

	if (result & 0x8000) result |= 0xFFFF0000;
	result >>= regs.R[cMem[regs.PC - 2] >> 4] & 0xF;
	regs.R[regID] = result & 0xFFFF;
	updateZandN(&regs.R[regID]);
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////PUSH INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//C0 0X 00 00	PUSH RX			Store register X on stack. Increase SP by 2.
void CPUCore::PUSHRX() {
	writeMemory(regs.R[cMem[regs.PC - 3] & 0xF], regs.SP);
	++regs.SP;
	writeMemory(regs.R[cMem[regs.PC - 3] & 0xF] >> 8, regs.SP);
	++regs.SP;
}

//C2 00 00 00	PUSHALL			Store all general purpose registers (r0~rF) at [SP]. Increase SP by 32.
void CPUCore::PUSHALL() {
	for (int i = 0; i < 16; ++i) {
		writeMemory(regs.R[i], regs.SP);
		++regs.SP;
		writeMemory(regs.R[i] >> 8, regs.SP);
		++regs.SP;
	}
}

//C4 00 00 00	PUSHF 			Store flags register on stack. Increase SP by 2.
void CPUCore::PUSHF() {
	writeMemory(regs.flag, regs.SP);
	regs.SP += 2;
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////POP INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//C1 0X 00 00	POP  RX			Decrease SP by 2. Load register X from stack. 
void CPUCore::POPRX() {
	regs.SP -= 2;
	regs.R[cMem[regs.PC - 3] & 0xF] = (cMem[regs.SP + 1] << 8) | cMem[regs.SP];
}

//C3 00 00 00	POPALL			Decrease SP by 32. Load all general purpose registers (r0~rF) from [SP].
void CPUCore::POPALL() {
	regs.SP -= 32;
	for (int i = 0; i < 32; i += 2) {
		regs.R[i >> 1] = (cMem[regs.SP + i + 1] << 8) | cMem[regs.SP + i];
	}
}

//C5 00 00 00	POPF			Decrease SP by 2. Load flags register from stack.
void CPUCore::POPF() {
	regs.SP -= 2;
	regs.flag = cMem[regs.SP];
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////FETCH,DECODE,EXECUTE INSTRUCTIONS//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


void CPUCore::fetchDecodeExecute(){
	opcode = cMem[regs.PC];
	regs.PC += 4;
	regs.PC &= 0xFFFF;
	++cycle;

	switch (opcode) {
		case 0x00:
			break;
		case 0x01:
			CLS();
			break;
		case 0x02:
			VBLNK();
			break;
		case 0x03:
			BGCN();
			break;
		case 0x04:
			SPRHHLL();
			break;
		case 0x05:
			DRWRXRYHHLL();
			break;
		case 0x06:
			DRWRXRYRZ();
			break;
		case 0x07:
			RNDRXHHLL();
			break;
		case 0x08:
			FLIP();
			break;
		case 0x09:
			//SND0();	
			break;
		case 0x0A:
			//SND1HHLL();
			break;
		case 0x0B:
			//SND2HHLL();
			break;
		case 0x0C:
			//SND3HHLL();
			break;
		case 0x0D:
			//SNPRxHHLL();
			break;
		case 0x0E:
			//SNGADVTSR();
			break;
		case 0x10:
			JMPHHLL();
			break;
		case 0x12:
			JxHHLL();
			break;
		case 0x13:
			JMERXRYHHLL();
			break;
		case 0x16:
			JMPRX();
			break;
		case 0x14:
			CALLHHLL();
			break;
		case 0x15:
			RET();
			break;
		case 0x17:
			CxHHLL();
			break;
		case 0x18:
			CALLRX();
			break;
		case 0x20:
			LDIRXHHLL();
			break;
		case 0x21:
			LDISPHHLL();
			break;
		case 0x22:
			LDMRXHHLL();
			break;
		case 0x23:
			LDMRXRY();
			break;
		case 0x24:
			MOVRXRY();
			break;
		case 0x30:
			STMRXHHLL();
			break;
		case 0x31:
			STMRXRY();
			break;
		case 0x40:
			ADDIRXHHLL();
			break;
		case 0x41:
			ADDRXRY();
			break;
		case 0x42:
			ADDRXRYRZ();
			break;
		case 0x50:
			SUBIRXHHLL();
			break;
		case 0x51:
			SUBRXRY();
			break;
		case 0x52:
			SUBRXRYRZ();
			break;
		case 0x53:
			CMPIRXHHLL();
			break;
		case 0x54:
			CMPRXRY();
			break;
		case 0x60:
			ANDIRXHHLL();
			break;
		case 0x61:
			ANDRXRY();
			break;
		case 0x62:
			ANDRXRYRZ();
			break;
		case 0x63:
			TSTIRXHHLL();
			break;
		case 0x64:
			TSTRXRY();
			break;
		case 0x70:
			ORIRXHHLL();
			break;
		case 0x71:
			ORRXRY();
			break;
		case 0x72:
			ORRXRYRZ();
			break;
		case 0x80:
			XORIRXHHLL();
			break;
		case 0x81:
			XORRXRY();	
			break;
		case 0x82:
			XORRXRYRZ();
			break;
		case 0x90:
			MULIRXHHLL();
			break;
		case 0x91:
			MULRXRY();	
			break;
		case 0x92:
			MULRXRYRZ();
			break;
		case 0xA0:
			DIVIRXHHLL();
			break;
		case 0xA1:
			DIVRXRY();
			break;
		case 0xA2:
			DIVRXRYRZ();
			break;
		case 0xB0:
			SHLRXN();
			break;
		case 0xB1:
			SHRRXN();	
			break;
		case 0xB2:
			SARRXN();
			break;
		case 0xB3:
			SHLRXRY();
			break;
		case 0xB4:
			SHRRXRY();
			break;
		case 0xB5:
			SARRXRY();
			break;
		case 0xC0:
			PUSHRX();
			break;
		case 0xC1:
			POPRX();	
			break;
		case 0xC2:
			PUSHALL();
			break;
		case 0xC3:
			POPALL();	
			break;
		case 0xC4:
			PUSHF(); 
			break;
		case 0xC5:
			POPF();
			break;
		case 0xD0:
			PALHHLL();
			break;
		case 0xD1:
			PALRx();
			break;
	}

	//Check for VBlank
	if (graphics.checkIfVBlank(&cycle)) {
		inputs.checkInputs(cMem);
		graphics.renderToScreen();
		cycle = 0;
	}
}